#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include "visualization_msgs/Marker.h"
#include <actionlib_msgs/GoalStatusArray.h>
#include <actionlib_msgs/GoalStatus.h>

#include "distance.hpp"
#include "dwa.hpp"

actionlib_msgs::GoalStatus last_goal;
std::shared_ptr<Dwa> dwa_ptr;

void path_callback(const nav_msgs::PathConstPtr& msg)
{
    auto global_plan = *msg;
    dwa_ptr->set_plan(global_plan);
}

void move_status_callback(const actionlib_msgs::GoalStatusArrayConstPtr& msg)
{
    if(dwa_ptr == nullptr || msg->status_list.size() == 0)
        return ;
    if(msg->status_list.back().goal_id.stamp != last_goal.goal_id.stamp || msg->status_list.back().status != last_goal.status)
    {
        last_goal = msg->status_list.back();
        dwa_ptr->set_status(last_goal.status==1);
    }
}



int main(int argc,char** argv)
{
    ros::init(argc,argv,"navigation");
    ros::NodeHandle n("~");
    std::string plan_topic;
    n.getParam("plan_topic",plan_topic);
    ros::Subscriber path_sub = n.subscribe(plan_topic,5,&path_callback);
    ros::Subscriber status_sub = n.subscribe("/move_base/status",5,&move_status_callback);
    dwa_ptr.reset(new Dwa(n));
    ros::spin();
}
